#include <utility>
#include <list>
#include <time.h>
#include <math.h>
#include "image_utils.cpp"
#include "nodo.h"
#include "solucion.h"
// #include<stdio.h>
// #include <iostream>
// #include <utility>
// #include <string>
// #include <stack>

using namespace std;
using namespace ImageUtils;

float F_Prof(Nodo &nodo_aux, pair<int,int> posFin){
  return -(nodo_aux.get_prof());
}

float F_Euclidea(Nodo &nodo_aux, pair<int,int> posFin){
  float dist, cost;
  dist = sqrt(pow((posFin.first - nodo_aux.get_coX()),2) +
	      pow((posFin.second - nodo_aux.get_coY()),2));
  cost = nodo_aux.get_cost() + dist;
  return cost;
}

double Val(Solucion solucion){
  double valoracion;

  valoracion = double(solucion.get_cost())/double(solucion.size_posiciones());

  return valoracion;
}

pair<int,int> Posicion_Cercana(Solucion sol_orig, pair<int,int> n_pos,
			       map<pair<int,int>, int> mapa){
  pair<int,int> pos_corte = sol_orig.get_posicion();
  pair<int,int> pos_aux;
  sol_orig.pop_posicion();
  int i,nodos_totales;
  nodos_totales = sol_orig.size_posiciones();
  double distancia_aux, distancia_corte;
  distancia_corte = sqrt(pow(n_pos.first-pos_corte.first,2) +
		       pow(n_pos.second-pos_corte.second,2)); //euclidea

  for(i=0;i<nodos_totales;i++){
    pos_aux = sol_orig.get_posicion();
    sol_orig.pop_posicion();
    distancia_aux = sqrt(pow(n_pos.first-pos_aux.first,2) +
			    pow(n_pos.second-pos_aux.second,2));
    if(distancia_aux<distancia_corte){
      distancia_corte = distancia_aux;
      pos_corte = pos_aux;
    }
  }
  return pos_corte;
}


bool Expandir(map<pair<int,int>, int> mapa_ref, map<pair<int,int>, int> &mapa,
	      int X, int Y, pair<int,int> max_dimension,
	      Nodo &nodo_actual, multimap<int,Nodo*> &frontera,
	      multimap<int,Nodo*> &visitados, pair<int,int> pos_fin,
	      int estrategia){
  bool expandido = false;
  float(*p_func)(Nodo&, pair<int,int>);
  float f=0;
  int posicion = 0;
  switch(estrategia){
    // case 1: // anchura
    //   p_func = F_Anchura;
    //   break;
    // case 2: // coste uniforme
    //   p_func = F_CosteU;
    //   break;
  case 3: // profundidad
    p_func = F_Prof;
    break;
  case 4: // A* Euclídea
    p_func = F_Euclidea;
    break;
    // case 5: // A* Manhattan
    //   p_func = F_Manhattan;
    //   break;
    // case 6: // profundidad acotada
    //   p_func = F_Prof;
    //   break;
    // case 7: // profundidad iterativa
    //   p_func = F_Prof;
    //   break;
  default:
    cout << "Opcion incorrecta en P_FUNC!!!!" << endl;
    return false;
  } //end case estrategia-funcion

  int aux_coste;
  if (X >= 0 && X <= max_dimension.first &&
      Y >= 0 && Y <= max_dimension.second){

    // cout <<"EXPAND: " << &nodo_actual << endl;

    if (mapa_ref[make_pair(X,Y)]==0)
      aux_coste = 100;
    else
      aux_coste = mapa_ref[make_pair(X,Y)];

    Nodo * aux_nodo = new Nodo(0,0,0,0,NULL,0);
    aux_nodo->set(X,Y,aux_coste,&nodo_actual);
    f = p_func(*aux_nodo, pos_fin);
    posicion = X+(Y*(pos_fin.first+1));
    if (mapa[make_pair(X,Y)]>=0){
      aux_nodo->set_f(f);
      frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
      mapa[make_pair(X,Y)] = -1;
      visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
      expandido = true;
    }
    else{
      if(mapa[make_pair(X,Y)]==-1){//visitados
	Nodo* nodo_ant = visitados.find(posicion)->second;
	if (aux_nodo->get_cost() < nodo_ant->get_cost()){
	  aux_nodo->set_f(f);
	  frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
	  visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
	  expandido = true;
	}
      }
    }

    // if (mapa[make_pair(X,Y)]<0){
    //   Nodo* nodo_ant = visitados.find(posicion)->second;
    //   if (aux_nodo->get_cost() < nodo_ant->get_cost()){
    // 	aux_nodo->set_f(f);
    // 	frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
    // 	visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
    // 	expandido = true;
    //   }
    // }
    // else{
    //   aux_nodo->set_f(f);
    //   frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
    //   mapa[make_pair(X,Y)] = -1;
    //   visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
    //   expandido = true;
    // }

  }//end if dentro márgenes
 return expandido;
}

void Crear_Solucion(Solucion &sol, Nodo &nodo){
  sol.set_cost(nodo.get_cost());
  Nodo* nodo_aux = &nodo;
  while(nodo_aux->get_father() != NULL){
    sol.set_posicion(make_pair(nodo_aux->get_coX(),nodo_aux->get_coY()));
    nodo_aux = nodo_aux->get_father();
  }
  sol.set_posicion(make_pair(nodo_aux->get_coX(),nodo_aux->get_coY()));
}


Solucion Busqueda(map<pair<int,int>, int> mapa,
		  pair<int,int> pos_ini, pair<int,int> pos_fin,
		  pair<int,int> max_dimension, int estrategia){
  Solucion sol;
  map<pair<int,int>, int> mapa_ref = mapa;
  int X,Y;
  int nodos_cont = 0;
  Nodo inicial(pos_ini.first,pos_ini.second,0,0,NULL,0);
  multimap<int,Nodo*> frontera;
  multimap<int,Nodo*> visitados;
  frontera.insert(pair<float,Nodo*>(inicial.get_f(), &inicial));
  visitados.insert(pair<int,Nodo*>(0, &inicial));

  while (!frontera.empty()) {
    Nodo* nodo_actual = frontera.begin()->second;
    frontera.erase(frontera.begin());
    if(nodo_actual->esSolucion(pos_fin)){
      Crear_Solucion(sol,*nodo_actual);
      break;
    }
    else{
      X = nodo_actual->get_coX();
      Y = nodo_actual->get_coY();
      // //////////////////// PEPE EL GUARRO ////////////////////
      // cout << "\t nodo_actual: " << nodo_actual << " ";
      // nodo_actual->toString();
      // cout << "\t nodos expandidos :" << nodos_cont << endl;
      // ////////////////////////////////////////////////////////
      /* expandimos  DCHA-ABAJO-IZQ-ARRIBA */
      if(Expandir(mapa_ref,mapa,X+1,Y,max_dimension,*nodo_actual,frontera,
		  visitados,pos_fin,estrategia))
	nodos_cont ++;
      if(Expandir(mapa_ref,mapa,X,Y+1,max_dimension,*nodo_actual,frontera,
		  visitados,pos_fin,estrategia))
	nodos_cont ++;
      if(Expandir(mapa_ref,mapa,X-1,Y,max_dimension,*nodo_actual,frontera,
		  visitados,pos_fin,estrategia))
	nodos_cont ++;
      if(Expandir(mapa_ref,mapa,X,Y-1,max_dimension,*nodo_actual,frontera,
		  visitados,pos_fin,estrategia))
	nodos_cont ++;
    }
  }//end while frontera no vacía
  return sol;
}

void Rellenar_Mapa(Solucion s1, map<pair<int,int>, int> &mapa){
  pair<int,int> pos_aux;
  while(!s1.empty_posiciones()){
    mapa[s1.get_posicion()] = 15;
    s1.pop_posicion();
  }
}

Solucion Vecina(Solucion solucion1, pair<int,int> n_pos, pair<int,int> pos_fin,
		map<pair<int,int>, int> mapa, pair<int,int> max_dimension,
		int estrategia){
  Solucion sol_Nposi, modif_sol;
  pair<int,int> pos_corte;

  map<pair<int,int>, int> mapa_aux = mapa;

  pos_corte = Posicion_Cercana(solucion1, n_pos, mapa);
  bool encontrada = solucion1.cortar(pos_corte,mapa);
  if (!encontrada){
    cout<< "ERROR en Vecina(solucion,n_pos,mapa)";
    cout << " n_pos no se encuentra en la solución" << endl;
  }
  else{
    //  cout<< "---------- > POS CORTE:" << pos_corte.first << " " << pos_corte.second << "   n_pos" << n_pos.first << " " <<n_pos.second <<endl;

    sol_Nposi = Busqueda(mapa,pos_corte,n_pos,max_dimension,estrategia);
    // Rellenar_Mapa(sol_Nposi, mapa_aux);
    // BasicImageUtils::PrintMap(mapa_aux,(char*)"sol2.pgm", 100, 100);
    // sol_Nposi.toString();

    solucion1.concatenar_sol(sol_Nposi.get_Pposiciones());
    // mapa_aux = mapa;
    // Rellenar_Mapa(solucion1, mapa_aux);
    // BasicImageUtils::PrintMap(mapa_aux,(char*)"sol3.pgm", 100, 100);
    // solucion1.toString();

    modif_sol = Busqueda(mapa,n_pos,pos_fin,max_dimension,estrategia);
    // mapa_aux = mapa;
    // Rellenar_Mapa(modif_sol, mapa_aux);
    // BasicImageUtils::PrintMap(mapa_aux,(char*)"sol4.pgm", 100, 100);
    // modif_sol.toString();

    solucion1.concatenar_sol(modif_sol.get_Pposiciones());
    // mapa_aux = mapa;
    // Rellenar_Mapa(solucion1, mapa_aux);
    // BasicImageUtils::PrintMap(mapa_aux,(char*)"sol5.pgm", 100, 100);
    // solucion1.toString();
  }
  return solucion1;
}

void C_Map(pair<int,int> p_cent, int radio, map<pair<int,int>, int> &mapa,
      pair<int,int> max_dimension){
  int x,y;
  for(y=p_cent.second-radio; y<=p_cent.second+radio; y++){
    for(x=p_cent.first-radio; x<=p_cent.first+radio; x++){
      if(x>=0 && y>=0 &&
	 x<=max_dimension.first && y<=max_dimension.second &&
	 pow(x-p_cent.first,2)<=pow(radio,2)-pow(y-p_cent.second,2) &&
	 pow(y-p_cent.second,2)<=pow(radio,2)-pow(x-p_cent.first,2)){
	mapa[make_pair(x,y)]=-2;
      }
    }
  }

}


Solucion Vecina2(Solucion sol_orig, pair<int,int> p_centro,
		 pair<int,int> pos_fin,	map<pair<int,int>, int> &mapa,
		 pair<int,int> max_dimension, int estrategia){
  Solucion sol_ret;
  int radio = rand() % max_dimension.first/2 + 1;// entre 1 y MAX_X/2

  C_Map(p_centro, radio, mapa, max_dimension);

  sol_ret=Busqueda(mapa,make_pair(0,0),pos_fin,max_dimension,estrategia);

  //Marca_C(mapa);
  return sol_ret;
}

int main(){
  int width, depth, estrategia;
  Solucion s1, modif_sol, modif_sol2;
  map<pair<int,int>, int> mapa; // first coordenadas(x,y) second valor
  map<pair<int,int>, int> mapa_m1; // first coordenadas(x,y) second valor
  map<pair<int,int>, int> mapa_m2; // first coordenadas(x,y) second valor

  BasicImageUtils::ImageDimensions((char*)"20x20.pgm",width,depth);
  pair<int,int> pos_ini = make_pair(0,0);
  pair<int,int> pos_fin = make_pair(width-1,depth-1);
  pair<int,int> max_dimension = make_pair(width-1,depth-1);
  mapa = BasicImageUtils::Map((char *)"20x20.pgm");
  mapa_m1 = mapa;
  mapa_m2 = mapa;
  estrategia = 4;

  pair<int,int> nueva_pos = make_pair(10,10);

  s1 = Busqueda(mapa,pos_ini,pos_fin, max_dimension, estrategia);
  Rellenar_Mapa(s1, mapa);
  BasicImageUtils::PrintMap(mapa,(char*)"sol1.pgm", width, depth);

  // cout << "sol1 encontrada: ";
  // s1.toString();


  modif_sol = Vecina(s1,nueva_pos,pos_fin,mapa_m1,max_dimension,estrategia);
  Rellenar_Mapa(modif_sol, mapa_m1);
  BasicImageUtils::PrintMap(mapa_m1,(char*)"sol_m1.pgm", width, depth);

  // cout<< "sol_m1 encontrada: ";
  // modif_sol.toString();

  // modif_sol2 = Vecina2(s1,nueva_pos,pos_fin,mapa_m2,max_dimension,estrategia);
  // Rellenar_Mapa(modif_sol2, mapa_m2);
  // BasicImageUtils::PrintMap(mapa_m2,(char*)"sol_m2b.pgm", width, depth);

  // cout<< "sol_m2 encontrada: ";
  // modif_sol2.toString();

  //PRUEBA1
  // Solucion s1;
  // pair<int,int> aux;
   // int x,y,nodos_totales;
  // //double aux_double;
  // //stack<pair<int,int> > posi;

  // //s1.set_cost(230);
  // for(y=10; y>0; y--){
  //   for(x=10; x>0; x--){
  //     aux.first = x;
  //     aux.second = y;
  //     s1.set_posicion(aux);
  //   }
  // }
    // nodos_totales = s1.size_posiciones();
    // cout << "Nodos totales: " << nodos_totales << " coste: " << s1.get_cost() << endl;
  // cout << "Valoracion: " << Val(s1) << endl;
  // for(x=0; x<nodos_totales ; x++){
  //     aux = s1.get_posicion();
  //     s1.pop_posicion();
  //     cout << "x: "<< aux.first << " y: " << aux.second << endl;
  // }

  return 0;
}
